#include <iostream>
#include <ros/ros.h>
#include "robot_control/Trajectory.h"
#include "robot_control/Odometry.h"
#include <fcntl.h> 
#include <termios.h>
#include <math.h>
#include "mathfunctions.h"

#include <gsl/gsl_rng.h>		// gsl library for random number generator
#include <gsl/gsl_cdf.h>		// gsl library for
#include <gsl/gsl_matrix.h>		// gsl library for matrix
#include <gsl/gsl_vector.h>		// gsl library for vector
#include <gsl/gsl_blas.h>		// gsl library for matrix operations
#include <gsl/gsl_linalg.h>	// gsl library for linear algebra
#include <gsl/gsl_randist.h>

double encoder_resolusion = 150;
double gearbox = 75;
double wheel_radius = 0.07;//m
double wheelbase = 0.4; 

double initial_enc0;
double initial_enc1;
bool initial = true;
double pos_old_x = 0;
double pos_old_y = 0;
double theta_old = 0;

pose2_t pre_odom;
FILE *fake_odometry_trajectory;
FILE *fake_encoder_trajectory;
 double previous_enc0;
  double previous_enc1;
int i=0;
matrix44_t action_model;
gsl_rng *rng_;



     
vector2_t pose2_add_pos(vector2_t a, pose2_t b) {
	double co, si;
	vector2_t c;
	co = cos(b.rot);
	si = sin(b.rot);
	c.x = co * a.x - si * a.y + b.pos.x;
	c.y = si * a.x + co * a.y + b.pos.y;
	return c;
}

/* Add two poses (rotation component). */
double pose2_add_rot(double a, pose2_t b) {
	double c;
	c = atan2(sin(a + b.rot), cos(a + b.rot));
	return c;
}


vector2_t pose2_sub_pos(vector2_t a, pose2_t b) {
	double co, si;
	vector2_t c;
	co = cos(b.rot);
	si = sin(b.rot);
	c.x = +co * (a.x - b.pos.x) + si * (a.y - b.pos.y);
	c.y = -si * (a.x - b.pos.x) + co * (a.y - b.pos.y);

	return c;
}

/* Subtract two poses (rotation component). */
double pose2_sub_rot(double a, pose2_t b) {
	double c;
	c = atan2(sin(a - b.rot), cos(a - b.rot));
	return c;
}


/* Add two poses. */
pose2_t pose2_add(pose2_t a, pose2_t b) {
	pose2_t c;
	c.pos = pose2_add_pos(a.pos, b);
	c.rot = pose2_add_rot(a.rot, b);
	return c;
}


/* Subtract poses: c = a - b. */
pose2_t pose2_sub(pose2_t a, pose2_t b) {
	pose2_t c;
	c.pos = pose2_sub_pos(a.pos, b);
	c.rot = pose2_sub_rot(a.rot, b);
	return c;
}

/** @brief Matrix/vector multiplication c = ab. */
vector4_t matrix44_mul_4(matrix44_t a, vector4_t b) {
	vector4_t c;
	c.x = +a.m[0][0] * b.x + a.m[0][1] * b.y + a.m[0][2] * b.z + a.m[0][3]
			* b.w;
	c.y = +a.m[1][0] * b.x + a.m[1][1] * b.y + a.m[1][2] * b.z + a.m[1][3]
			* b.w;
	c.z = +a.m[2][0] * b.x + a.m[2][1] * b.y + a.m[2][2] * b.z + a.m[2][3]
			* b.w;
	c.w = +a.m[3][0] * b.x + a.m[3][1] * b.y + a.m[3][2] * b.z + a.m[3][3]
			* b.w;
	return c;
}


pose2_t ApplyActionModel(pose2_t delta) {

	vector4_t sigma;
	pose2_t offset;

	sigma.x = fabs(delta.pos.x);
	sigma.y = fabs(delta.pos.y);
	sigma.z = fabs(delta.rot);
	sigma.w = 1.0;

	sigma = matrix44_mul_4(action_model, sigma);

		offset = delta;

		offset.pos.x += gsl_ran_gaussian(rng_, sigma.x);
		offset.pos.y += gsl_ran_gaussian(rng_, sigma.y);
		offset.rot += gsl_ran_gaussian(rng_, sigma.z);

		//sample->pose = pose2_add(offset, sample->pose);

	return offset;
}


robot_control::Trajectory pose;
//using output angular position
void ComputeOdometryCallback(const robot_control::Odometry::ConstPtr& msg)//check enc0 is left?
{
  ROS_INFO("i hear enc0 %ld,enc1 %ld", (long int) msg->enc0, (long int) msg->enc1);
  
  double angular_position[2];
  double line_position[2];
  double fractpart[2], intpart[2];
 
  double delta_enc0, delta_enc1;
  double delta_distance;
  pose2_t delta;
  rng_ = gsl_rng_alloc(gsl_rng_taus);
  pose2_t offset;
  
  if(initial){
     previous_enc0 = msg->enc0;
     previous_enc1 = msg->enc1;
     initial = false;
   }
  delta_enc0 = msg->enc0 - previous_enc0;
  delta_enc1 = msg->enc1 - previous_enc1;
  
  action_model.m[0][0] = 0.00;
	action_model.m[2][0] = 0.00;
	action_model.m[2][2] = 0.8;//0.05
  
  
  if(delta_enc0 != 0 && delta_enc1 != 0 ){
     //fprintf(fake_encoder_trajectory, "%d enc0 %ld enc1 %ld \n", i, current_enc0, current_enc1);
	   //fflush(fake_encoder_trajectory);
	   previous_enc0 = msg->enc0;
	   previous_enc1 = msg->enc1;

  //ROS_INFO("current_enc0 %ld,current_enc1 %ld", (long int)current_enc0, (long int)current_enc1);
 
  angular_position[0] = (double) delta_enc0 / (encoder_resolusion * gearbox) ;
  angular_position[1] = (double) delta_enc1 / (encoder_resolusion * gearbox);
  fractpart[0]= modf(angular_position[0], &intpart[0]) ;
  fractpart[1]= modf(angular_position[1], &intpart[1]) ;
 // line_position[0] = 2 * M_PI * wheel_radius * fractpart[0] + 2 * M_PI * intpart[0];
 // line_position[1] = 2 * M_PI * wheel_radius * fractpart[1] + 2 * M_PI * intpart[1];
   line_position[0] =  wheel_radius * angular_position[0] * 2 * M_PI ;
   line_position[1] =  wheel_radius * angular_position[1] * 2 * M_PI; 
   
  delta_distance = (line_position[0] + line_position[1])/2;
  
  delta.rot =(line_position[0] - line_position[1])/wheelbase;//1-0 or 0-1
  
  pose.rot += delta.rot;
  delta.pos.x = delta_distance * cos(pose.rot);
  delta.pos.y = delta_distance * sin(pose.rot);
  
  offset =  ApplyActionModel(delta);
  
  pose.x += offset.pos.x;
  pose.y += offset.pos.y;
  pose.rot = pose.rot - delta.rot + offset.rot;
 
  ROS_INFO("pos_x %f,pos_y %f, theta %f", pose.x, pose.y, pose.rot);
   }
  
 
  
}


int main (int argc, char** argv)
{
	ros::init(argc, argv, "planaer_tester");
	ros::NodeHandle nh;
  ros::Subscriber odo_sub = nh.subscribe("robot_odometry",1000, ComputeOdometryCallback);
  ros::Publisher odo_pub = nh.advertise<robot_control::Trajectory>(
			"robot_trajectory", 10);
			
	  ros::Rate loop_rate(10);
		robot_control::Trajectory pose;
		pose.x = 0;
		pose.y = 0;
		pose.priority = 3;
	double x = 0;
	double y = 0;
	ROS_INFO("Published msg");
	while(ros::ok()) {
		robot_control::Trajectory msg;
		msg.x = x;
		msg.y = y;
		x += 0.01;
		y += 0.01;
		odo_pub.publish(msg);
		loop_rate.sleep();
		ROS_INFO("Published msg");
	}
   
   while(ros::ok()){
    odo_pub.publish(pose);
    //ROS_INFO("pos_x %f,pos_y %f", pose.x, pose.y);
     loop_rate.sleep();
     ros::spinOnce();
    //ROS_INFO("Published pose odometry");
   }
}
